package org.opentcs.kcvehicle.communication.kc.udp.Service;

import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEvent;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kcvehicle.communication.kc.udp.io.UDPClient;

public class QryRobotStatus
    extends
      BaseCommand {


  /**
   * decs: 查询机器人状态
   * 指令：0xAF
   * author: caixiang
   * date: 2025/1/17 16:25
   */
  public static AgvEvent queryStatus() {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS);
    return agvEvent;
  }

  public static QueryRobotStatusRsp command() {
    //0xAF(查询机器人状态)
    AgvEvent agvEvent = queryStatus();
    printInfo(agvEvent);
    RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
    if(rcv.isOk()){
      System.out.println();
      for (byte b:rcv.getValue()){
        System.out.print(byteToHex(b)+" ");
      }
      System.out.println();
      System.out.println("0xAF received transationId : "+ "isok:"+rcv.isOk());

//      for (byte c:rcv.getDataBytes()){
//        System.out.print(byteToHex(c)+" ");
//      }
      QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
      return queryRobotStatusRsp;
    }
    else {
      System.out.println();
      System.out.println("received transationId : " + "isok:" + rcv.isOk());
      throw new RuntimeException("0xAF fail");
    }
  }

}
